#include "filter.h"
#include "lpfconfig.h"
#include "hpfconfig.h"

#include <algorithm>

#include <QDebug>
#include <QVector>

#define OLD_ALGORITHM 0

PairArray Filter::gainArray(const DMVariant &dm)
{
    PairArray array;
    std::visit(
        overloaded {
            [&array](const EQ &eq) {
                switch (eq.type) {
                case EQType::PEQ:
                    array = Filter::peq(eq);
                    break;
                case EQType::Lo_Shelf:
                case EQType::Hi_Shelf:
                    array = Filter::hleq(eq);
                    break;
                case EQType::ALLPASS_90:
                    array = ap90();
                    break;
                case EQType::ALLPASS_180:
                    array = ap180();
                    break;
                default:
                    array = PairArray();
                    break;
                }
            },
            [&array](const Xover &xover) {
                switch (xover.type) {
                case XoverType::Hpf:
                    array = Filter::hpf(xover);
                    break;
                case XoverType::Lpf:
                    array = Filter::lpf(xover);
                    break;
                default:
                    array = PairArray();
                    break;
                }
            }
        }, dm);
    return array;
}

double Filter::freqToWidth(const double &freq)
{
    // 处理边界
    if (freq <= EQ_FREQUENCY_MIN) {
        return 0.0;
    }

    if (freq >= EQ_FREQUENCY_MAX) {
        return 1.0;
    }

    // 对频率进行对数转换（使用常用对数log10更符合音频频率感知）
    double logFreq = std::log10(freq);
    double logMin = std::log10(EQ_FREQUENCY_MIN);
    double logMax = std::log10(EQ_FREQUENCY_MAX);

    // 将对数频率线性映射到总宽度范围内
    return (logFreq - logMin) / (logMax - logMin);
}

double Filter::gainToHeight(const double &gain)
{
    return (EQ_GAIN_MAX - gain) / EQ_GAIN_LENGTH;
}

double Filter::widthToFreq(const double &w, const double &width)
{
    if (width <= 0.0) {
        return EQ_FREQUENCY_MIN;
    }

    if (width >= w) {
        return EQ_FREQUENCY_MAX;
    }
    double logMin = std::log10(EQ_FREQUENCY_MIN);
    double logMax = std::log10(EQ_FREQUENCY_MAX);
    double logFreq = logMin + (width / w) * (logMax - logMin);

    return std::pow(10, logFreq);
}

double Filter::heightToGain(const double &h, const double &height)
{
    if (std::abs(h) < EPSILON_12E) {
        return EQ_GAIN_DEFAULT;
    }
    const double gain = EQ_GAIN_MAX - EQ_GAIN_LENGTH * height / h;
    return std::clamp(gain, EQ_GAIN_MIN, EQ_GAIN_MAX);
}

double Filter::bwToQ(const int &bw)
{
    if (bw < 0) {
        return EQ_Q.front();
    }
    if (bw >= EQ_Q_COUNT) {
        return EQ_Q.back();
    }
    return EQ_Q.at(bw);
}

int Filter::qToBw(const double &q)
{
    int left = 0;
    int right = static_cast<int>(EQ_Q.size() - 1);

    while (left <= right) {
        int mid = left + (right - left) / 2;
        auto midVal = EQ_Q[mid];

        if (midVal == q) {
            return mid;
        } else if (midVal > q) {
            left = mid + 1;
        } else {
            right = mid - 1;
        }
    }
    return right;
}

PairArray Filter::peq(const EQ &eq)
{
    size_t nFreq = getFreqIndex(eq.freq);  //查找索引值
    double PEQ_Fc = EQ_FREQUENCY[nFreq];
    double PEQ_dB = getdB(eq.gain, eq.type);
    double PEQ_bw = getBW(eq.bw);
    // double QQ = 1 / (2 * sinh(log(2) / 2 * (PEQ_bw)));
    const double w0 = FilterConfig::FS_PI * PEQ_Fc;

    const double temp = PEQ_dB / 40.0;
    const double temp_A = pow(10.0, temp);
    const double temp_sin = sin(w0);
    const double alpha = temp_sin * sinh(log(2) * PEQ_bw * w0 / temp_sin);
    double temp_a0 = 1+alpha/temp_A;
    const double temp_a1 = (-2) * cos(w0);
    const double temp_a2 = 1 - alpha / temp_A;
    const double temp_b0 = 1 + alpha * temp_A;
    const double temp_b1 = (-2) * cos(w0);
    const double temp_b2 = 1 - alpha * temp_A;
    temp_a0 = 1.0 / temp_a0;
    const double a1 = temp_a1 * temp_a0;//a1
    const double a2 = temp_a2 * temp_a0;//a2
    const double b0 = temp_b0 * temp_a0;//b0
    const double b1 = temp_b1 * temp_a0; //b1
    const double b2 = temp_b2 * temp_a0; //b2

    return gainArray(a1, a2, b0, b1, b2);
}

PairArray Filter::hleq(const EQ &eq)
{
    const auto &type = eq.type;
    const double &fc = eq.freq;
    const double SEQ_dB = getdB(eq.gain, eq.type);
    const double w0 = FilterConfig::FS_PI * fc;

    constexpr double temp_S = 1.0;
    const double temp = SEQ_dB / 40.0;
    const double temp_A = pow(10.0, temp);
    const double temp_cos = cos(w0);
    const double alpha = sin(w0) / 2 * sqrt((temp_A + 1 / temp_A) * (1 / temp_S - 1) + 2);
    const double temp_pp = 2 * sqrt(temp_A) * alpha;
    // SEQ_dB == 0 时的值
    // 归一化后的IIR滤波器分母系数（二阶）
    double a1 = 0, a2 = 0;
    // 归一化后的IIR滤波器分子系数
    double b0 = 1.0, b1 = 0.0, b2 = 0.0;
    if (SEQ_dB != 0) {
        if (EQType::Lo_Shelf == type) {
            const double tempa = (temp_A + 1) + (temp_A - 1) * temp_cos + temp_pp;
            a1 = ((-2) * ((temp_A - 1) + (temp_A + 1) * temp_cos)) / tempa;
            a2 = ((temp_A + 1) + (temp_A - 1) * temp_cos - temp_pp) / tempa;
            b0 = (temp_A * ((temp_A + 1) - (temp_A - 1) * temp_cos + temp_pp)) / tempa;
            b1 = (2 * temp_A * ((temp_A - 1) - (temp_A + 1) * temp_cos)) / tempa;
            b2 = (temp_A * ((temp_A + 1) - (temp_A - 1) * temp_cos - temp_pp)) / tempa;
        }
        else if (EQType::Hi_Shelf == type) {
            const double tempa =(temp_A + 1) - (temp_A - 1) * temp_cos + temp_pp;
            a1 = (2 * ((temp_A - 1) - (temp_A + 1) * temp_cos)) / tempa;
            a2 = ((temp_A + 1) - (temp_A - 1) * temp_cos - temp_pp) / tempa;
            b0 = (temp_A * ((temp_A + 1) + (temp_A - 1) * temp_cos + temp_pp)) / tempa;
            b1 = ((-2) * temp_A * ((temp_A - 1) + (temp_A + 1) * temp_cos)) / tempa;
            b2 = (temp_A * ((temp_A + 1) + (temp_A - 1) * temp_cos - temp_pp)) / tempa;
        }
    }
    return gainArray(a1, a2, b0, b1, b2);
}

PairArray Filter::ap90()
{
    PairArray array;
    for (size_t i = 0; i < array.size(); ++i) {
        array[i] = EQ_GAIN_DEFAULT;
    }
    return array;
}

PairArray Filter::ap180()
{
    return ap90();
}

PairArray Filter::hpf(const Xover &xover)
{
    if (xover.freq <= EQ_FREQUENCY_MIN || xover.freq > EQ_FREQUENCY_MAX) {
        return PairArray();
    }

#if OLD_ALGORITHM
    BiquadCoeffs coeffs = HpfConfig::calculateCoeffs(xover.freq, xover.ftype, xover.slope);

    return hlpf(coeffs);
#else
    PairArray array;
    const auto n = order(xover.ftype, xover.slope);
    switch (xover.ftype) {
    case PfType::ButterWorth:
        for (int i = 0; i < EQ_FREQUENCY_COUNT; ++i) {
            array[i] = gain(butter_worth_hpf(n, xover.freq, EQ_FREQUENCY[i]));
        }
        break;
    case PfType::LinkwitzRiley:
        for (int i = 0; i < EQ_FREQUENCY_COUNT; ++i) {
            array[i] = gain(linkwitz_riley_hpf(n, xover.freq, EQ_FREQUENCY[i]));
        }
        break;
    case PfType::Bessel:
        return bessel_hpf(xover.slope, xover.freq);
        break;
    default:
        break;
    }
    return array;

#endif
}

PairArray Filter::lpf(const Xover &xover)
{
    if (xover.freq < EQ_FREQUENCY_MIN || xover.freq >= EQ_FREQUENCY_MAX) {
        return PairArray();
    }

#if !OLD_ALGORITHM
    BiquadCoeffs coeffs;
    if (xover.ftype == PfType::Bessel) {
        coeffs = LpfConfig::BesselFilter::calculateCoeffs(xover.freq, xover.slope);
    } else {
        coeffs = LpfConfig::LrButterFilter::calculateCoeffs(xover.freq, xover.ftype, xover.slope);
    }
    return hlpf(coeffs);
#else
    PairArray array;
    const auto n = order(xover.ftype, xover.slope);
    switch (xover.ftype) {
    case PfType::ButterWorth:
        for (int i = 0; i < EQ_FREQUENCY_COUNT; ++i) {
            array[i] = gain(butter_worth_hpf(n, xover.freq, EQ_FREQUENCY[i]));
        }
        break;
    case PfType::LinkwitzRiley:
        for (int i = 0; i < EQ_FREQUENCY_COUNT; ++i) {
            array[i] = gain(linkwitz_riley_lpf(n, xover.freq, EQ_FREQUENCY[i]));
        }
        break;
    case PfType::Bessel:
        return bessel_lpf(xover.slope, xover.freq);
        break;
    default:
        break;
    }
    return array;

#endif
}

PairArray Filter::hlpf(const BiquadCoeffs &coeffs)
{
    PairArray response;
    for (auto j = 0; j < EQ_FREQUENCY_COUNT; ++j) {
        double totalGainDb = 0.0; // 累计增益（dB），每次频点重置为0
        for (auto i = 0; i < COEFFS_SIZE; ++i) { // 遍历4个二阶节，叠加增益
            // 系数归一化（除以a0，避免重复计算）
            const double a1 = coeffs.a1[i] / coeffs.a0[i];
            const double a2 = coeffs.a2[i] / coeffs.a0[i];
            const double b0 = coeffs.b0[i] / coeffs.a0[i];
            const double b1 = coeffs.b1[i] / coeffs.a0[i];
            const double b2 = coeffs.b2[i] / coeffs.a0[i];
            totalGainDb += calculateGain(EQ_FREQUENCY.at(j), a1, a2, b0, b1, b2);
        }

        response[j] = totalGainDb;
    }

    return response;
}

PairArray Filter::gainArray(const double &a1, const double &a2, const double &b0, const double &b1, const double &b2)
{
    PairArray array;
    for (size_t i = 0; i < EQ_FREQUENCY.size(); ++i) {
        array[i] = calculateGain(EQ_FREQUENCY.at(i), a1, a2, b0, b1, b2);
    }
    return array;
}

double Filter::calculateGain(const double &freq, const double &a1, const double &a2, const double &b0, const double &b1, const double &b2)
{
    const double PEQ_Wf = FilterConfig::FS_PI * freq;
    const double A = cos(PEQ_Wf) + a1 + a2 * cos(PEQ_Wf);
    const double B = sin(PEQ_Wf) - a2 * sin(PEQ_Wf);
    const double C = b0 * cos(PEQ_Wf) + b1 + b2 * cos(PEQ_Wf);
    const double D = b0 * sin(PEQ_Wf) - b2 * sin(PEQ_Wf);
    const double EE = sqrt((C * C + D * D) / (A * A + B * B));

    return 20*log10l(EE);
}

double Filter::getdB(const double &gain, const EQType &type)
{
    switch (type) {
    case EQType::ALLPASS_90:
    case EQType::ALLPASS_180:
        return EQ_GAIN_DEFAULT;
    default:
        return gain;
    }
}

double Filter::getBW(const uint16_t &bw)
{
    return bw * 0.01 + 0.01;
}

size_t Filter::getFreqIndex(const double &target)
{
    if (EQ_FREQUENCY.empty()) {
        return 0;
    }
    // 边界处理
    if (target <= EQ_FREQUENCY.front()) {
        return 0;
    }
    if (target >= EQ_FREQUENCY.back()) {
        return EQ_FREQUENCY.size() - 1;
    }

    size_t low = 0;
    size_t high = EQ_FREQUENCY.size() - 1;

    while (low < high) {
        size_t mid = low + (high - low) / 2;

        if (EQ_FREQUENCY[mid] == target) {
            return mid;
        }
        else if (EQ_FREQUENCY[mid] < target) {
            low = mid + 1;
        }
        else {
            high = mid;
        }
    }
    if (target - EQ_FREQUENCY[low - 1] <= EQ_FREQUENCY[low] - target) {
        return low - 1;  // 前一个元素更近
    } else {
        return low;  // 当前元素更近
    }
}

uint8_t Filter::order(PfType type, Slope slope)
{
    switch (type) {
    case PfType::Bessel:
        return (slope + 1) * 2;
    case PfType::ButterWorth:
        return (slope + 1) * 2;
    case PfType::LinkwitzRiley:
        return (slope + 1);
    }
    return 0;
}

double Filter::gain(double amplitude)
{
    return 20 * std::log10(amplitude);
}

double Filter::butter_worth_lpf(uint8_t n, uint16_t fc, double f)
{
    return (1 / std::sqrt(1 + std::pow(f / fc, 2 * n)));
}

double Filter::butter_worth_hpf(uint8_t n, uint16_t fc, double f)
{
    const double rn = std::pow(f / fc, n);
    return rn / std::sqrt(1 + std::pow(rn, 2));
}

double Filter::linkwitz_riley_lpf(uint8_t n, uint16_t fc, double f)
{
    return std::pow(butter_worth_lpf(n, fc, f), 2);
}

double Filter::linkwitz_riley_hpf(uint8_t n, uint16_t fc, double f)
{
    return std::pow(butter_worth_hpf(n, fc, f), 2);
}

cd Filter::bessel_hpf_polynomial(int order, const cd &s)
{
    switch(order) {
    case 2:  return 1.0 + 3.0*s + 3.0*s*s;
    case 4:  return 1.0 + 10.0*s + 45.0*s*s + 105.0*pow(s,3) + 105.0*pow(s,4);
    case 6:  return 1.0 + 21.0*s + 210.0*s*s + 1260.0*pow(s,3) + 4725.0*pow(s,4) + 10395.0*pow(s,5) + 10395.0*pow(s,6);
    case 8:  return 1.0 + 36.0*s + 630.0*s*s + 6930.0*pow(s,3) + 51975.0*pow(s,4) + 270270.0*pow(s,5) + 945945.0*pow(s,6) + 2027025.0*pow(s,7) + 2027025.0*pow(s,8);
    default: return 0.0;
    }
}

cd Filter::bessel_lpf_polynomial(int order, const cd &s)
{
    switch(order) {
    case 2:  return 3.0 + 3.0*s + 1.0*s*s;
    case 4:  return 105.0 + 105.0*s + 45.0*s*s + 10.0*pow(s,3) + 1.0*pow(s,4);
    case 6:  return 10395.0 + 10395.0*s + 4725.0*s*s + 1260.0*pow(s,3) + 210.0*pow(s,4) + 21.0*pow(s,5) + 1.0*pow(s,6);
    case 8:  return 2027025.0 + 2027025.0*s + 945945.0*s*s + 270270.0*pow(s,3) + 51975.0*pow(s,4) + 6930.0*pow(s,5) + 630.0*pow(s,6) + 36.0*pow(s,7) + 1.0*pow(s,8);
    default: return 0.0;
    }
}

double Filter::bessel_hpf_magnitude_squared(int order, double x)
{
    cd s(0.0, x);  // 构造虚数 s = j*x（实部0，虚部x）
    cd bessel = bessel_hpf_polynomial(order, s);  // 计算复数多项式值
    return norm(bessel);  // norm = 模值平方（实部² + 虚部²），等价于abs(bessel)^2
}

double Filter::bessel_lpf_magnitude_squared(int order, double x)
{
    cd s(0.0, x);
    cd bessel = bessel_lpf_polynomial(order, s);
    return norm(bessel);
}

double Filter::bessel_hpf_gain(int order, double designCutoff, double freq)
{
    if (freq <= 0) return -1000.0;

    double x = designCutoff / freq;  // 归一化参数（设计截止频率/当前频率）
    double magSq = bessel_hpf_magnitude_squared(order, x);
    return gain(1.0 / sqrt(magSq));
}

double Filter::bessel_lpf_gain(int order, double designCutoff, double freq)
{
    if (freq >= 24000) return -1000.0;

    double x = freq / designCutoff;
    double magSq = bessel_lpf_magnitude_squared(order, x);
    return gain(1.0 / sqrt(magSq));
}

double Filter::bessel_hpf_design_cutoff(int order, double targetFreq)
{
    double low = 0.01 * targetFreq;
    double high = 100.0 * targetFreq;
    double eps = 1e-8;
    double targetGain = -3.0;

    for (int i = 0; i < 200; ++i) {
        double mid = (low + high) / 2;
        double gain = bessel_hpf_gain(order, mid, targetFreq);

        if (gain > targetGain) low = mid;
        else high = mid;

        if (high - low < eps) break;
    }

    return (low + high) / 2;
}

double Filter::bessel_lpf_design_cutoff(int order, double targetFreq)
{
    double low = 0.01 * targetFreq;
    double high = 100.0 * targetFreq;
    double eps = 1e-8;
    double targetGain = -3.0;

    for (int i = 0; i < 200; ++i) {
        double mid = (low + high) / 2;
        double gain = bessel_lpf_gain(order, mid, targetFreq);

        if (gain > targetGain) low = mid;
        else high = mid;

        if (high - low < eps) break;
    }

    return (low + high) / 2;
}

PairArray Filter::bessel_hpf(Slope slope, uint16_t fc)
{
    const int n = order(PfType::Bessel, slope);
    const auto cutoff = bessel_hpf_design_cutoff(n, fc);
    PairArray array;
    for (int i = 0; i < EQ_FREQUENCY_COUNT; ++i) {
        array[i] = bessel_hpf_gain(n, cutoff, EQ_FREQUENCY.at(i));
    }
    return array;
}

PairArray Filter::bessel_lpf(Slope slope, uint16_t fc)
{
    const int n = order(PfType::Bessel, slope);
    const auto cutoff = bessel_lpf_design_cutoff(n, fc);
    PairArray array;
    for (int i = 0; i < EQ_FREQUENCY_COUNT; ++i) {
        array[i] = bessel_lpf_gain(n, cutoff, EQ_FREQUENCY.at(i));
    }
    return array;
}




